/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package javatest1;

/**
 *
 * @author paul
 */
public class RoboManipulator extends RoboTask {
    static double[] pegHeights = {12, 18, 20, 21, 22, 25, 30};

    static double maxHeight = 30;
    static double baseHeight = 12;
    static double ticksPerInch = 10;
    static double bufferHeight = 4;
    static double heightAccuracy = 0.5;

    static double ticksPerDegree = 10;
    static double bufferAngle = 5;
    static double angleAccuracy = 1;

    double currentHeight = baseHeight;
    double targetHeight = baseHeight;
    double currentElevatorPower = 0;

    double currentAngle = 0;
    double targetAngle = 0;
    double currentArmPower = 0;

    boolean armMoving = false;
    boolean elevatorMoving = false;
    
    private boolean trace = false;
    private double elevSpeedScale = 6.0;   // max speed in/sec
    private double armSpeedScale = 30.0;   // max speed deg/sec


    void goToPeg(int number){
        moveElevatorTo(pegHeights[number]);
        moveArmTo((number != 0)?0:100);
    }

    void moveElevatorTo(double height){
        targetHeight = height;
        elevatorMoving = (Math.abs(currentHeight - targetHeight) >= heightAccuracy);
    }

    void moveArmTo(double angle){
        targetAngle = angle;
        armMoving = (Math.abs(currentAngle - targetAngle) >= angleAccuracy);
    }

    boolean isMoving(){
        return (armMoving || elevatorMoving);
    }

    void handle(){
        super.handle();


        // read height & angle

        // adjust elevator height
        double heightDifference = Math.abs(targetHeight - currentHeight);
        double direction = Math.signum(targetHeight - currentHeight);
        if(heightDifference > bufferHeight) {
            // fast down
            moveElevator(direction);
        }
        else if(heightDifference > heightAccuracy) {
            // gradually slow down
            moveElevator(direction * ((heightDifference / bufferHeight) * 0.5 + 0.5));
        }
        else {
            // stop
            moveElevator(0);
        }

        // adjust arm angle
        double angleDifference = Math.abs(targetAngle - currentAngle);
        double armDirection = Math.signum(targetAngle - currentAngle);
        if(angleDifference > bufferAngle) {
            // fast down
            moveArm(armDirection);
        }
        else if(angleDifference > angleAccuracy) {
            // gradually slow down
            moveArm(armDirection * ((angleDifference / bufferAngle) * 0.5 + 0.5));
        }
        else {
            // stop
            moveArm(0);
        }
    }
    void moveElevator(double power){
        if(power == currentElevatorPower) return;
        currentElevatorPower = power;
        elevatorMoving = (power != 0);
        if(trace) System.out.printf("T=%1.2f: Elevator at %1.2f, moving %1.1f\n", taskTime, currentHeight, power);
    }
    void moveArm(double power){
        if(power == currentArmPower) return;
        currentArmPower = power;
        armMoving = (power != 0);
        if(trace) System.out.printf("T=%1.2f: Arm at %1.2f, moving %1.1f\n", taskTime, currentAngle, power);
    }

    void simulate(){
        double dt = robot.simDt;

        // simulate motion
        currentHeight  += currentElevatorPower * elevSpeedScale * dt;
        // simulate motion
        currentAngle  += currentArmPower * armSpeedScale * dt;
    }
}
